/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package deatax.math;

import java.util.Arrays;

/**
 *
 * @author pson
 */
public class Matrix4 {
    
    /**
     * 0  1  2  3
     * 4  5  6  7
     * 8  9  10 11
     * 12 13 14 15
     */
    public float[] v;

    public Matrix4() {
        v = new float[] {
            1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1
        };
    }
    

    public Matrix4(float[] val) {
        this.v = val;
    }
    
    public Matrix4(Matrix3 m) {
        float mv[] = m.v;
        v = new float[] {
            mv[0], mv[1], mv[2], 0.0f,
            mv[3], mv[4], mv[5], 0.0f,
            mv[6], mv[7], mv[8], 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f
        };
        
    }
    
    public Matrix4(Matrix4 m) {
        this(m.v);
    }
    
    public boolean equals(Matrix4 m) {
        return Arrays.equals(v, m.v);
    }
    
    public Matrix4 multiply(Matrix4 m) {
        float mv[] = m.v;
        return new Matrix4(new float[] {
            mv[0] * v[0] + mv[4] * v[1] + mv[8] * v[2] + mv[12] * v[3],
            mv[1] * v[0] + mv[5] * v[1] + mv[9] * v[2] + mv[13] * v[3],
            mv[2] * v[0] + mv[6] * v[1] + mv[10] * v[2] + mv[14] * v[3],
            mv[3] * v[0] + mv[7] * v[1] + mv[11] * v[2] + mv[15] * v[3],
            
            mv[0] * v[4] + mv[4] * v[5] + mv[8] * v[6] + mv[12] * v[7],
            mv[1] * v[4] + mv[5] * v[5] + mv[9] * v[6] + mv[13] * v[7],
            mv[2] * v[4] + mv[6] * v[5] + mv[10] * v[6] + mv[14] * v[7],
            mv[3] * v[4] + mv[7] * v[5] + mv[11] * v[6] + mv[15] * v[7],
            
            mv[0] * v[8] + mv[4] * v[9] + mv[8] * v[10] + mv[12] * v[11],
            mv[1] * v[8] + mv[5] * v[9] + mv[9] * v[10] + mv[13] * v[11],
            mv[2] * v[8] + mv[6] * v[9] + mv[10] * v[10] + mv[14] * v[11],
            mv[3] * v[8] + mv[7] * v[9] + mv[11] * v[10] + mv[15] * v[11],
            
            mv[0] * v[12] + mv[4] * v[13] + mv[8] * v[14] + mv[12] * v[15],
            mv[1] * v[12] + mv[5] * v[13] + mv[9] * v[14] + mv[13] * v[15],
            mv[2] * v[12] + mv[6] * v[13] + mv[10] * v[14] + mv[14] * v[15],
            mv[3] * v[12] + mv[7] * v[13] + mv[11] * v[14] + mv[15] * v[15]
        });
    }
    
    public Vector3 multiply(Vector3 vec) {
        return new Vector3(v[0] * vec.x + v[1] * vec.y + v[2] * vec.z + v[3],
                           v[4] * vec.x + v[5] * vec.y + v[6] * vec.z + v[7],
                           v[8] * vec.x + v[9] * vec.y + v[10] * vec.z + v[11]);
    }
    
    public Matrix4 transposed() {
        return new Matrix4(new float[]{
            v[0], v[4], v[8], v[12],
            v[1], v[5], v[9], v[13],
            v[2], v[6], v[10], v[14],
            v[3], v[7], v[11], v[15]
        });
    }
    
    public float determinant() {
        return v[0]*v[5]*v[10]*v[15] - v[0]*v[5]*v[11]*v[14] + v[0]*v[6]*v[11]*v[13] - v[0]*v[6]*v[9]*v[15]
             + v[0]*v[7]*v[9]*v[14]  - v[0]*v[7]*v[10]*v[13] - v[1]*v[6]*v[11]*v[12] + v[1]*v[6]*v[8]*v[15]
             - v[1]*v[7]*v[8]*v[14]  + v[1]*v[7]*v[10]*v[12] - v[1]*v[4]*v[10]*v[15] + v[1]*v[4]*v[11]*v[14] 
             + v[2]*v[7]*v[8]*v[13]  - v[2]*v[7]*v[9]*v[12]  + v[2]*v[4]*v[9]*v[15]  - v[2]*v[4]*v[11]*v[13] 
             + v[2]*v[5]*v[11]*v[12] - v[2]*v[5]*v[8]*v[15]  - v[3]*v[4]*v[9]*v[14]  + v[3]*v[4]*v[10]*v[13]
             - v[3]*v[5]*v[10]*v[12] + v[3]*v[5]*v[8]*v[14]  - v[3]*v[6]*v[8]*v[13]  + v[3]*v[6]*v[9]*v[12];
    }
    
    public Matrix4 inversed() {
        float d = determinant();
        if (d == 0) {
            System.err.println("Cannot invert matrix!");
            return null;
        }
        d = 1.0f / d;
        
        return new Matrix4(new float[] {
            d  * (v[5] * (v[10] * v[15] - v[11] * v[14]) + v[6] * (v[11] * v[13] - v[9] * v[15]) + v[7] * (v[9] * v[14] - v[10] * v[13])),
            -d * (v[1] * (v[10] * v[15] - v[11] * v[14]) + v[2] * (v[11] * v[13] - v[9] * v[15]) + v[3] * (v[9] * v[14] - v[10] * v[13])),
            d  * (v[1] * (v[6] * v[15] - v[7] * v[14]) + v[2] * (v[7] * v[13] - v[5] * v[15]) + v[3] * (v[5] * v[14] - v[6] * v[13])),
            -d * (v[1] * (v[6] * v[11] - v[7] * v[10]) + v[2] * (v[7] * v[9] - v[5] * v[11]) + v[3] * (v[5] * v[10] - v[6] * v[9])),
            -d * (v[4] * (v[10] * v[15] - v[11] * v[14]) + v[6] * (v[11] * v[12] - v[8] * v[15]) + v[7] * (v[8] * v[14] - v[10] * v[12])),
            d  * (v[0] * (v[10] * v[15] - v[11] * v[14]) + v[2] * (v[11] * v[12] - v[8] * v[15]) + v[3] * (v[8] * v[14] - v[10] * v[12])),
            -d * (v[0] * (v[6] * v[15] - v[7] * v[14]) + v[2] * (v[7] * v[12] - v[4] * v[15]) + v[3] * (v[4] * v[14] - v[6] * v[12])),
            d  * (v[0] * (v[6] * v[11] - v[7] * v[10]) + v[2] * (v[7] * v[8] - v[4] * v[11]) + v[3] * (v[4] * v[10] - v[6] * v[8])),
            d  * (v[4] * (v[9] * v[15] - v[11] * v[13]) + v[5] * (v[11] * v[12] - v[8] * v[15]) + v[7] * (v[8] * v[13] - v[9] * v[12])),
            -d * (v[0] * (v[9] * v[15] - v[11] * v[13]) + v[1] * (v[11] * v[12] - v[8] * v[15]) + v[3] * (v[8] * v[13] - v[9] * v[12])),
            d  * (v[0] * (v[5] * v[15] - v[7] * v[13]) + v[1] * (v[7] * v[12] - v[4] * v[15]) + v[3] * (v[4] * v[13] - v[5] * v[12])),
            -d * (v[0] * (v[5] * v[11] - v[7] * v[9]) + v[1] * (v[7] * v[8] - v[4] * v[11]) + v[3] * (v[4] * v[9] - v[5] * v[8])),
            -d * (v[4] * (v[9] * v[14] - v[10] * v[13]) + v[5] * (v[10] * v[12] - v[8] * v[14]) + v[6] * (v[8] * v[13] - v[9] * v[12])),
            d  * (v[0] * (v[9] * v[14] - v[10] * v[13]) + v[1] * (v[10] * v[12] - v[8] * v[14]) + v[2] * (v[8] * v[13] - v[9] * v[12])),
            -d * (v[0] * (v[5] * v[14] - v[6] * v[13]) + v[1] * (v[6] * v[12] - v[4] * v[14]) + v[2] * (v[4] * v[13] - v[5] * v[12])),
            d  * (v[0] * (v[5] * v[10] - v[6] * v[9]) + v[1] * (v[6] * v[8] - v[4] * v[10]) + v[2] * (v[4] * v[9] - v[5] * v[8]))
        });
    }

    public void decompose(Vector3 scale, Quaternion rotate, Vector3 pos) {
        pos.x = v[3];
        pos.y = v[7];
        pos.z = v[11];
        
        Vector3 r1 = new Vector3(v[0], v[4], v[8]);
        Vector3 r2 = new Vector3(v[1], v[5], v[9]);
        Vector3 r3 = new Vector3(v[2], v[6], v[10]);
        
        scale.x = r1.length();
        scale.y = r2.length();
        scale.z = r3.length();
        
        if (scale.x != 0.0f) {
            r1.divide(scale.x);
        }
        if (scale.y != 0.0f) {
            r2.divide(scale.y);
        }
        if (scale.z != 0.0f) {
            r3.divide(scale.z);
        }
        
        Matrix3 m = new Matrix3(new float[] {
                r1.x, r2.x, r3.x,
                r1.y, r2.y, r3.y,
                r1.z, r2.z, r3.z});
        
        rotate.set(m);
        
    }
    
    public void decomposeNoScale(Quaternion rotate, Vector3 pos) {
        pos.x = v[3];
        pos.y = v[7];
        pos.z = v[11];
        
        rotate.set(new Matrix3(this));
    }
    
    public boolean isIdentity() {
        final float e = 10e-3f;
        return (v[1] <= e && v[1] >= -e &&
                v[2] <= e && v[2] >= -e &&
                v[3] <= e && v[3] >= -e &&
                v[4] <= e && v[4] >= -e &&
                v[6] <= e && v[6] >= -e &&
                v[7] <= e && v[7] >= -e &&
                v[8] <= e && v[8] >= -e &&
                v[9] <= e && v[9] >= -e &&
                v[11] <= e && v[11] >= -e &&
                v[12] <= e && v[12] >= -e &&
                v[13] <= e && v[13] >= -e &&
                v[14] <= e && v[14] >= -e &&
                v[0] <= 1.0f+e && v[0] >= 1.0f-e &&
                v[5] <= 1.0f+e && v[5] >= 1.0f-e &&
                v[10] <= 1.0f+e && v[10] >= 1.0f-e &&
                v[15] <= 1.0f+e && v[15] >= 1.0f-e);
    }
    
    public static Matrix4 fromEuler(Vector3 vec) {
        return fromEuler(vec.x, vec.y, vec.z);
    }
    
    public static Matrix4 fromEuler(float x, float y, float z) {
        float cx = (float)Math.cos(x);
        float sx = (float)Math.sin(x);
        float cy = (float)Math.cos(y);
        float sy = (float)Math.sin(y);
        float cz = (float)Math.cos(z);
        float sz = (float)Math.sin(z);
        
        float sxsy = sx * sy;
        float cxsy = cx * sy;
        
        return new Matrix4(new float[] {
            cy*cz, cy*sz, -sy, 0.0f,
            sxsy*cz-cx*sz, sxsy*sz+cx*cz, sx*cy, 0.0f,
            cxsy*cz+sx*sz, cxsy*sz-sx*cz, cx*cy, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f
        });
        
    }
    
    public static Matrix4 rotationX(float a) {
        final float s = (float)Math.sin(a);
        final float c = (float)Math.cos(a);
        
        return new Matrix4(new float[]{
            1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, c,    -s,   0.0f,
            0.0f, s,    c,    0.0f,
            0.0f, 0.0f, 0.0f, 1.0f
        });
    }
    
    public static Matrix4 rotationY(float a) {
        final float s = (float)Math.sin(a);
        final float c = (float)Math.cos(a);
        
        return new Matrix4(new float[]{
            c,    0.0f, s,    0.0f,
            0.0f, 1.0f, 0.0f, 0.0f,
            -s,   0.0f, c,    0.0f,
            0.0f, 0.0f, 0.0f, 1.0f
        });
    }
    
    public static Matrix4 rotationZ(float a) {
        final float s = (float)Math.sin(a);
        final float c = (float)Math.cos(a);
        
        return new Matrix4(new float[]{
            c,    -s,   0.0f, 0.0f,
            s,    c,    0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f
        });
    }
    
    public static Matrix4 rotation(float a, Vector3 axis) {
        final float c = (float)Math.cos(a);
        final float s = (float)Math.sin(a);
        final float t = 1.0f - c;
        
        final float x = axis.x;
        final float y = axis.y;
        final float z = axis.z;
        
        return new Matrix4(new float[]{
            t*x*x+c,   t*x*y-s*z, t*x*z+s*y, 0.0f,
            t*x*y+s*z, t*y*y+c,   t*y*z-s*x, 0.0f,
            t*x*z-s*y, t*y*z+s*x, t*z*z+c,   0.0f,
            0.0f,      0.0f,      0.0f,      1.0f
        });
    }
    
    public static Matrix4 translation(Vector3 vec) {
        Matrix4 m = new Matrix4();
        m.v[3] = vec.x;
        m.v[7] = vec.y;
        m.v[11] = vec.z;
        return m;
    }
    
    public static Matrix4 scale(Vector3 vec) {
        Matrix4 m = new Matrix4();
        m.v[0] = vec.x;
        m.v[5] = vec.y;
        m.v[10] = vec.z;
        return m;
    }
    
    public static Matrix4 fromToMatrix(Vector3 from, Vector3 to) {
        return new Matrix4(Matrix3.fromToMatrix(from, to));
    }
}
